{---------------------------------

        K U D U

----------------------------------}



#define _debug_

CON
  _CLKMODE = XTAL1 + PLL16X
  _XINFREQ = 5_000_000

OBJ
  ROSC : "ROSC"
  Servo : "Servo32v7"
  CPE   : "CPE"
  RL    : "radioloop"




VAR
  byte joint[ROSC#MAX_SERVOS]    ' what kind of joint is it: left, right, hip, knee, shoulder
  byte servo_pin[ROSC#MAX_SERVOS]
  byte cog
  long direction
  long rx_a, rx_b, rx_c

PUB Init

  ' load pins
  CPE.SetServoCount(12)  
  CPE.Init 
  CPE.LoadServoSettings           ' get the servo settings
  CPE.SetDirection(0)  ' set controls forward

  CPE.SetSpeed(CLKFREQ/180)' 360)' 5760)' 2880) ' Zero to test set up position CLKFREQ/100)         ' freeze
  CPE.SetCycleStride(0, 0)



  cog:=CPE.StartGo



  CPE.SetDIrection(ROSC#RIGHT_FWD & ROSC#LEFT_BWD)


  repeat ' infinite loop

  repeat


    waitcnt(cnt+CLKFREQ*5)


    CPE.SetDirection(0)  ' set controls forward


    waitcnt(cnt+CLKFREQ*5)

    CPE.SetDIrection(ROSC#RIGHT_FWD)

    waitcnt(cnt+CLKFREQ*5)

    CPE.SetDIrection(ROSC#LEFT_FWD)


    waitcnt(cnt+CLKFREQ*3)

    CPE.SetDIrection(ROSC#DIR_FWD)

    waitcnt(cnt+CLKFREQ*3)

    CPE.SetDIrection(ROSC#DIR_BWD)



' No radio in kudu
'  RL.Init(13,14,15)

'  Control


PUB Control

  'ReadChannels
  'RCDirection
  'CPE.SetDirection(ROSC#DIR_FWD)


  repeat
    ReadChannels
     if (rx_a < 850) and (rx_a > 650)
      if (rx_b>850)
        CPE.SetDirection(ROSC#DIR_FWD)
      else
       if (rx_b<600)
        CPE.SetDirection(ROSC#DIR_BWD)
       else
         CPE.SetDirection(0)
     else
      if (rx_a > 850)
        CPE.SetDirection(ROSC#LEFT_FWD)
      else
        CPE.SetDirection(ROSC#RIGHT_FWD)


{
  waitcnt(cnt+CLKFREQ*10)
  CPE.SetDirection(0)
  waitcnt(cnt+CLKFREQ*10)

  CPE.SetDirection(ROSC#DIR_BWD)
  repeat




}




PUB ReadChannels


    rx_a:=RL.GetCurrent(0)


    rx_b:=RL.GetCurrent(1)


    rx_c:=RL.GetCurrent(2) ' pulseWidths[0]


PUB RCDirection


  ' in the center
    if (rx_a> 650) & (rx_a<850)
      CPE.SetSpeed(CLKFREQ/ 360)

      if (rx_b< 600)
        CPE.SetDirection(ROSC#DIR_FWD)
        CPE.SetSpeed(CLKFREQ/ 360)
      ELSE
        if (rx_b> 900)
           CPE.SetDirection(ROSC#DIR_BWD)
           CPE.SetSpeed(CLKFREQ/ 360)
        else
          CPE.SetSpeed(0)
    else
       CPE.SetSpeed(CLKFREQ/120)  ' slower while turning
       if (rx_a< 650)          ' left
         CPE.SetDirection(ROSC#LEFT_FWD | ROSC#RIGHT_BWD)
         if (rx_b< 600)
             CPE.SetDirection(ROSC#LEFT_FWD)
         if (rx_b>900)
             CPE.SetDirection(ROSC#LEFT_BWD)
       else ' right
         CPE.SetDirection(ROSC#RIGHT_FWD | ROSC#LEFT_BWD)
         if (rx_b<600)
            CPE.SetDirection(ROSC#RIGHT_FWD)
         if (rx_b>600)
            CPE.SetDirection(ROSC#RIGHT_BWD)

